from deepbots.robots.controllers.robot_emitter_receiver_csv import RobotEmitterReceiverCSV

class KcgRobot(RobotEmitterReceiverCSV):
    """
    KcgRobot consists of two identical car bodies. There are two Reconfigurable wheels in every car body.
    The robot cantains four linear motors, four rotation motors, IMU and GPS.  
    """
    def __init__(self):
        '''
        The constructor gets the Position Sensor reference and enables it and also initializes the wheels.
        '''
        super().__init__()
        # self.gps = self.robot.getDevice("gps")
        self.gps = self.robot.getGPS("gps")
        self.gps.enable(self.get_timestep())
        # self.IMU = self.robot.getDevice("inertial unit")
        self.IMU = self.robot.getInertialUnit("inertial unit")
        self.IMU.enable(self.get_timestep())

        self.wheels = [None for _ in range(8)]
        self.setup_motor()

    def setup_motor(self):
        """
        This method initializes the four wheels, storing the references inside a list and setting the starting
        positions and velocities.
        """
        self.wheels[0] = self.robot.getMotor('left_advance_1 motor')
        self.wheels[1] = self.robot.getMotor('right_advance_1 motor')
        self.wheels[2] = self.robot.getMotor('left_advance_2 motor')
        self.wheels[3] = self.robot.getMotor('right_advance_2 motor')
        self.wheels[4] = self.robot.getMotor('left_rotate_1 motor')
        self.wheels[5] = self.robot.getMotor('right_rotate_1 motor')
        self.wheels[6] = self.robot.getMotor('left_rotate_2 motor')
        self.wheels[7] = self.robot.getMotor('right_rotate_2 motor')
        for i in range(len(self.wheels)):
            self.wheels[i].setPosition(float('inf'))
            self.wheels[i].setVelocity(0.0)
    
    def create_message(self):
        '''        
        This method packs the robot's observation into a list of strings to be sent to the supervisor.
        The message contains only the Position Sensor value, ie. the angle from vertical position in radians.
        From Webots documentation:
        'The getValue function returns the most recent value measured by the specified position sensor. Depending on
        the type, it will return a value in radians (angular position sensor) or in meters (linear position sensor).'

        :return: A list of strings with the robot's observations.
        :rtype: list
        '''
        message1 = [str(self.wheels[0].getVelocity())]
        message2 = [str(self.wheels[1].getVelocity())]
        message3 = [str(self.wheels[2].getVelocity())]
        message4 = [str(self.wheels[3].getVelocity())]
        message5 = [str(self.wheels[4].getVelocity())]
        message6 = [str(self.wheels[5].getVelocity())]
        message7 = [str(self.wheels[6].getVelocity())]
        message8 = [str(self.wheels[7].getVelocity())]

        message9 = [str(self.gps.getValues()[0]), str(self.gps.getValues()[1]), str(self.gps.getValues()[2])]
        message10 = [str(self.IMU.getRollPitchYaw()[0]), str(self.IMU.getRollPitchYaw()[1]), str(self.IMU.getRollPitchYaw()[2])]
        
        message = message1 + message2 + message3 + message4 + message5 + message6 + message7 + message8 + message9 + message10
        # print("robot message: ", message)

        return message

    def use_message_data(self, message):
        '''
        This method unpacks the supervisor's message, which contains the next action to be executed by the robot.
        In this case it contains an integer denoting the action, either 0 or 1, with 0 being forward and
        1 being backward movement. The corresponding motorSpeed value is applied to the wheels.

        :param message: The message the supervisor sent containing the next action.
        :type message: list of strings
        '''
        move = float(message[0])
        rotation1 = float(message[1])
        rotation2 = float(message[2])

        for i in range(4):
            self.wheels[i].setPosition(float('inf'))
            self.wheels[i].setVelocity(move)
        
        self.wheels[4].setPosition(float('inf'))
        self.wheels[4].setVelocity(rotation1)
        self.wheels[5].setPosition(float('inf'))
        self.wheels[5].setVelocity(rotation1)

        self.wheels[6].setPosition(float('inf'))
        self.wheels[6].setVelocity(rotation2)
        self.wheels[7].setPosition(float('inf'))
        self.wheels[7].setVelocity(rotation2)
        

# Create the robot controller object and run it
robot_controller = KcgRobot()
robot_controller.run()